/*****************************************************************//**
 * \file   Num6.h
 * \brief  Num6
 *
 * \author Wyatt
 * \date   January 2022
 *********************************************************************/
#ifndef _CODE_LIB_NUM6_H
#define _CODE_LIB_NUM6_H
#include <math.h>

template<typename Type> struct SNum6
{
	SNum6<Type>(const Type& v0 = Type(0.), const Type& v1 = Type(0.), const Type& v2 = Type(0.),
		const Type& v3 = Type(0.), const Type& v4 = Type(0.), const Type& v5 = Type(0.))
	{
		v[0] = v0;
		v[1] = v1;
		v[2] = v2;
		v[3] = v3;
		v[4] = v4;
		v[5] = v5;
	}
	SNum6<Type>(const Type* v)
	{
		v[0] = v[0];
		v[1] = v[1];
		v[2] = v[2];
		v[3] = v[3];
		v[4] = v[4];
		v[5] = v[5];
	}
	union {
		struct { Type x; Type y; Type z; Type xy; Type yz; Type zx; };
		Type v[6];
	};
	template<typename> friend struct SNum6;
	SNum6<Type>& operator=(const SNum6<Type> &src)
	{		
		v[0] = src.v[0];
		v[1] = src.v[1];
		v[2] = src.v[2];
		v[3] = src.v[3];
		v[4] = src.v[4];
		v[5] = src.v[5];
		return *this;
	}
	SNum6<Type>& operator+=(const SNum6<Type> &src)
	{		
		v[0] += src.v[0];
		v[1] += src.v[1];
		v[2] += src.v[2];
		v[3] += src.v[3];
		v[4] += src.v[4];
		v[5] += src.v[5];
		return *this;
	}	
	SNum6<Type>& operator-=(const SNum6<Type> &src)
	{		
		v[0] -= src.v[0];
		v[1] -= src.v[1];
		v[2] -= src.v[2];
		v[3] -= src.v[3];
		v[4] -= src.v[4];
		v[5] -= src.v[5];
		return *this;
	}
	SNum6<Type>& operator*=(Type src)
	{		
		v[0] *= src;
		v[1] *= src;
		v[2] *= src;
		v[3] *= src;
		v[4] *= src;
		v[5] *= src;
		return *this;
	}
	SNum6<Type> operator+(const SNum6<Type> &src)
	{		
		SNum6<Type> tmp = *this;
		tmp+=src;
		return tmp;
	}
	SNum6<Type> operator-(const SNum6<Type> &src)
	{		
		SNum6<Type> tmp = *this;
		tmp-=src;
		return tmp;
	}
	SNum6<Type> operator*(Type src)
	{		
		SNum6<Type> tmp = *this;
		tmp*=src;
		return tmp;
	}
	operator const Type*() const 
	{
		return v;
	}
	SNum6<Type> & setVal(const Type v[6])
	{
		v[0] = v[0];
		v[1] = v[1];
		v[2] = v[2];
		v[3] = v[3];
		v[4] = v[4];
		v[5] = v[5];
		return *this;
	}
	SNum6<Type> & setVal(Type a_x, Type a_y, Type a_z,
		Type a_xy, Type a_yz, Type a_zx)
	{
		v[0] = a_x;
		v[1] = a_y;
		v[2] = a_z;
		v[3] = a_xy;
		v[4] = a_yz;
		v[5] = a_zx;
		return *this;
	}
	SNum6<Type>& setValAll(Type a_)
	{
		v[0] = a_;
		v[1] = a_;
		v[2] = a_;
		v[3] = a_;
		v[4] = a_;
		v[5] = a_;
		return *this;
	}
	const Type * value() const
	{
		return &v[0];
	}
	Type dot(const SNum6<Type> & a_vec) const
	{
		return (v[0]*a_vec[0] + v[1]*a_vec[1] + v[2]*a_vec[2],
			v[3]*a_vec[3] + v[4]*a_vec[4] + v[5]*a_vec[5]);
	}
	Type len() const
	{
		return (Type)sqrt(sqrLength());
	}
	Type sqrLength() const
	{
		return (v[0]*v[0])+(v[1]*v[1])+(v[2]*v[2])+
			(v[3]*v[3])+(v[4]*v[4])+(v[5]*v[5]);
	}
	SNum6<Type> normalize()
	{
		Type magnitude = len();

		if(magnitude != 0.0)
			(*this) *= (Type)(1.0 / magnitude);
		else setVal(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);

		return *this;
	}
	Type &  operator[](int i) { return v[i]; }
	const Type & operator[](int i) const { return v[i]; }
	Type* operator* ()
	{
		return v;
	}
	SNum6<Type> & operator /=(const Type d)
	{
		*this *= (1.0f/d);
		return *this;
	}
	SNum6<Type> & operator *=(const SNum6<Type> & a_vec)
	{
		v[0] *= a_vec.v[0];
		v[1] *= a_vec.v[1];
		v[2] *= a_vec.v[2];
		v[3] *= a_vec.v[3];
		v[4] *= a_vec.v[4];
		v[5] *= a_vec.v[5];
		return *this;
	}
	SNum6<Type> operator-() const
	{
		return SNum6<Type>(-v[0], -v[1], -v[2], -v[3], -v[4], -v[5]);
	}
	friend SNum6<Type> operator *(const SNum6<Type> & a_vec, const Type d)
	{ 
		return SNum6<Type>(a_vec.v[0] * d, a_vec.v[1] * d, a_vec.v[2] * d,
			a_vec.v[3] * d, a_vec.v[4] * d, a_vec.v[5] * d);
	}
	friend SNum6<Type> operator *(const Type d, const SNum6<Type> & a_vec)
	{ 
		return a_vec * d; 
	}
	friend SNum6<Type> operator /(const SNum6<Type> & a_vec, const Type d)
	{ 
		return SNum6<Type>(a_vec.v[0] / d, a_vec.v[1] / d, a_vec.v[2] / d,
			a_vec.v[3] / d, a_vec.v[4] / d, a_vec.v[5] / d);
	}

	friend SNum6<Type> operator *(const SNum6<Type> & v1, const SNum6<Type> & v2)
	{	
		return SNum6<Type>(v1.v[0] * v2.v[0],v1.v[1] * v2.v[1],v1.v[2] * v2.v[2],
			v1.v[3] * v2.v[3],v1.v[4] * v2.v[4],v1.v[5] * v2.v[5]);
	}

	friend SNum6<Type> operator +(const SNum6<Type> & v1, const SNum6<Type> & v2)
	{	
		return SNum6<Type>(v1.v[0] + v2.v[0],v1.v[1] + v2.v[1],v1.v[2] + v2.v[2],
			v1.v[3] + v2.v[3],v1.v[4] + v2.v[4],v1.v[5] + v2.v[5]);
	}

	friend SNum6<Type> operator -(const SNum6<Type> & v1, const SNum6<Type> & v2)
	{	
		return SNum6<Type>(v1.v[0] - v2.v[0],v1.v[1] - v2.v[1],v1.v[2] - v2.v[2],
			v1.v[3] - v2.v[3],v1.v[4] - v2.v[4],v1.v[5] - v2.v[5]);	
	}
	friend bool operator ==(const SNum6<Type> & v1, const SNum6<Type> & v2)
	{ 
		return (v1.v[0]==v2.v[0] && v1.v[1]==v2.v[1] && v1.v[2]==v2.v[2] &&
			v1.v[3]==v2.v[3] && v1.v[4]==v2.v[4] && v1.v[5]==v2.v[5]); 
	}
	friend bool operator !=(const SNum6<Type> & v1, const SNum6<Type> & v2)
	{ 
		return !(v1 == v2); 
	}
	bool isZero() const
	{
		return (x == 0 && y == 0 && z == 0 && xy == 0 && yz == 0 && zx == 0);
	}
	void negate()
	{
		setVal(-v[0], -v[1], -v[2], -v[3], -v[4], -v[5]);
	}
	bool equal(const SNum6<Type> & a_vec, const Type dTol=1E-2) const
	{
		Type dx = v[0] - a_vec.v[0];
		Type dy = v[1] - a_vec.v[1];
		Type dz = v[2] - a_vec.v[2];
		Type dxy = v[3] - a_vec.v[3];
		Type dyz = v[4] - a_vec.v[4];
		Type dzx = v[5] - a_vec.v[5];
		return ((dx*dx + dy*dy + dz*dz +
			dxy*dxy + dyz*dyz + dzx*dzx) <= dTol);
	}
};

typedef SNum6<double> SNum6d;

#endif // _CODE_LIB_NUM6_H